#!/usr/bin/env python3
# coding:utf-8
""" 
1.使用tf变化, 求turtle2 相对于 turtle1 的位置
2.根据x,y距离, 
    计算角度, 速度值
    控制turtle2 朝向turtle1 移动
"""

import math

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from tf2_ros import TransformException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener
from turtlesim.srv import Spawn


class FrameListener(Node):

    def __init__(self):
        super().__init__("turtle_tf2_frame_listener")

        # Declare and acquire `target_frame` parameter
        self.declare_parameter("target_frame", "turtle1")
        self.target_frame = (
            self.get_parameter("target_frame").get_parameter_value().string_value
        )

        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)

        # Create a client to spawn a turtle
        self.spawner = self.create_client(Spawn, "spawn")
        # Boolean values to store the information
        # if the service for spawning turtle is available
        self.turtle_spawning_service_ready = False
        # if the turtle was successfully spawned
        self.turtle_spawned = False

        # Create turtle2 velocity publisher
        self.publisher = self.create_publisher(Twist, "turtle2/cmd_vel", 1)

        # Call on_timer function every second
        self.timer = self.create_timer(1.0, self.on_timer)

    def on_timer(self):
        """
        ros2 topic ehco /tf 查看tf树
            有两个child_frame_id: turtle1, turtle2
        """
        from_frame_rel = self.target_frame
        to_frame_rel = "turtle2"

        if self.turtle_spawning_service_ready:
            if self.turtle_spawned:
                # Look up for the transformation between target_frame and turtle2 frames
                # and send velocity commands for turtle2 to reach target_frame
                try:
                    now = rclpy.time.Time()
                    trans = self.tf_buffer.lookup_transform(
                        to_frame_rel,
                        from_frame_rel,
                        now,
                        # timeout=Duration(seconds=1.0)
                    )
                except TransformException as ex:
                    self.get_logger().info(
                        f"Could not transform {to_frame_rel} to {from_frame_rel}: {ex}"
                    )
                    return

                msg = Twist()
                scale_rotation_rate = 1.0
                msg.angular.z = scale_rotation_rate * math.atan2(
                    trans.transform.translation.y, trans.transform.translation.x
                )

                scale_forward_speed = 0.5
                msg.linear.x = scale_forward_speed * math.sqrt(
                    trans.transform.translation.x**2 + trans.transform.translation.y**2
                )
                self.get_logger().info(f"偏差 x={trans.transform.translation.x},y={trans.transform.translation.y}")

                self.publisher.publish(msg)
            else:
                if self.result.done():
                    self.get_logger().info(
                        f"Successfully spawned {self.result.result().name}"
                    )
                    self.turtle_spawned = True
                else:
                    self.get_logger().info("Spawn is not finished")
        else:
            if self.spawner.service_is_ready():
                # Initialize request with turtle name and coordinates
                # Note that x, y and theta are defined as floats in turtlesim/srv/Spawn
                request = Spawn.Request()
                request.name = "turtle2"
                request.x = float(4)
                request.y = float(2)
                request.theta = float(0)
                # Call request
                self.result = self.spawner.call_async(request)
                self.turtle_spawning_service_ready = True
            else:
                # Check if the service is ready
                self.get_logger().info("Service is not ready")


def main():
    rclpy.init()
    node = FrameListener()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass

    rclpy.shutdown()


if __name__ == "__main__":
    main()
